/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include "attitude.h"
#include "pid.h"
#include "fc_sensors.h"
#include "estimator.h"
#include "imu.h"
#include "motor_mix.h"
#include "maths.h"
#include "rc_controls.h"
#include "motor_mix.h"
#include "fc_state.h"
#include "anoPTv8_report.h"
#include "position.h"

#define DBG_TAG "pos.pos"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

#define POSITION_P_SCALE 0.01f
#define POSITION_I_SCALE 0.01f
#define POSITION_D_SCALE 0.01f
#define MAX_VELOCITY_Z 200 // cm/s

ZINO_CONFIG_REGISTER(struct posHoldPidConfig, posHoldPidConfig, 1, 0);

const pidf_t pos_pid_default[POS_PID_COUNT] = {
    {0.0f, 0.0f, 0.0f},
    {0.0f, 0.0f, 0.0f},
    {0.0f, 0.0f, 0.0f},
    {0.0f, 0.0f, 0.0f},
};

pidf_t pos_pid_param[POS_PID_COUNT];
PID_t pos_pid_obj[POS_PID_COUNT];
static uint32_t manaulTx = 0;
static uint32_t manaulTy = 0;

void position_xy_pid_init(void)
{
    rt_memset(&pos_pid_param, 0, sizeof(pos_pid_param));
    if (posHoldPidConfig()->flag == POS_SAVED_FLAG)
    {
        rt_memcpy(&pos_pid_param, &posHoldPidConfig()->pid, sizeof(pos_pid_param));
    }
    else
    {
        rt_memcpy(&pos_pid_param, &pos_pid_default, sizeof(pos_pid_param));
        rt_memcpy(&posHoldPidConfig()->pid, &pos_pid_param, sizeof(pos_pid_param));
    }
    pid_set_parameters(&pos_pid_obj[POS_VX],
                       pos_pid_param[POS_VX].P * POSITION_P_SCALE,
                       pos_pid_param[POS_VX].I * POSITION_I_SCALE,
                       pos_pid_param[POS_VX].D * POSITION_D_SCALE,
                       20, 30);
    pid_set_parameters(&pos_pid_obj[POS_VY],
                       pos_pid_param[POS_VY].P * POSITION_P_SCALE,
                       pos_pid_param[POS_VY].I * POSITION_I_SCALE,
                       pos_pid_param[POS_VY].D * POSITION_D_SCALE,
                       20, 30);
    pid_set_parameters(&pos_pid_obj[POS_PX],
                       pos_pid_param[POS_PX].P * POSITION_P_SCALE,
                       pos_pid_param[POS_PX].I * POSITION_I_SCALE,
                       pos_pid_param[POS_PX].D * POSITION_D_SCALE,
                       100, 200);
    pid_set_parameters(&pos_pid_obj[POS_PY],
                       pos_pid_param[POS_PY].P * POSITION_P_SCALE,
                       pos_pid_param[POS_PY].I * POSITION_I_SCALE,
                       pos_pid_param[POS_PY].D * POSITION_D_SCALE,
                       100, 200);

    pos_pid_obj[POS_VX].mode = PID_MODE_DERIVATIV_CALC;
    pos_pid_obj[POS_VY].mode = PID_MODE_DERIVATIV_CALC;
    pos_pid_obj[POS_PX].mode = PID_MODE_DERIVATIV_CALC;
    pos_pid_obj[POS_PY].mode = PID_MODE_DERIVATIV_CALC;
}
void position_xy_pid_save(void)
{
    posHoldPidConfig()->flag = POS_SAVED_FLAG;
}
void position_xy_pid_reload(void)
{
    if (posHoldPidConfig()->flag == POS_SAVED_FLAG)
    {
        rt_memcpy(&pos_pid_param, &posHoldPidConfig()->pid, sizeof(pos_pid_param));
        for (int8_t i = 0; i < POS_PID_COUNT; i++)
        {
            pos_pid_obj[i].kp = pos_pid_param[i].P * POSITION_P_SCALE;
            pos_pid_obj[i].ki = pos_pid_param[i].I * POSITION_I_SCALE;
            pos_pid_obj[i].kd = pos_pid_param[i].D * POSITION_D_SCALE;
        }

        LOG_D("pos pid reload");
        LOG_D("VX: %3d, I: %3d, D: %3d", pos_pid_param[POS_VX].P, pos_pid_param[POS_VX].I, pos_pid_param[POS_VX].D);
        LOG_D("VY: %3d, I: %3d, D: %3d", pos_pid_param[POS_VY].P, pos_pid_param[POS_VY].I, pos_pid_param[POS_VY].D);
        LOG_D("PX: %3d, I: %3d, D: %3d", pos_pid_param[POS_PX].P, pos_pid_param[POS_PX].I, pos_pid_param[POS_PX].D);
        LOG_D("PY: %3d, I: %3d, D: %3d", pos_pid_param[POS_PY].P, pos_pid_param[POS_PY].I, pos_pid_param[POS_PY].D);
    }
}
void position_xy_pid_reset_default(void)
{
    rt_memcpy(&pos_pid_param, &pos_pid_default, sizeof(pos_pid_param));
    rt_memcpy(&posHoldPidConfig()->pid, &pos_pid_param, sizeof(pos_pid_param));
}
void position_xy_control(float dt)
{
    float vel_x_setpoint = getRcDeflection(PITCH) * rc_control_max()->level;
    float vel_y_setpoint = getRcDeflection(ROLL) * rc_control_max()->level; // REVERSE rc ROLL[-1,1] -> VEL_Y[1,-1]

    bool vel_x_manual = (ABS(vel_x_setpoint) > 1.f) ? 1 : 0;
    bool vel_y_manual = (ABS(vel_y_setpoint) > 1.f) ? 1 : 0;
    if (vel_x_manual)
    {
        fc_setpoint.manual[X] = true;
        fc_setpoint.angle.pitch = vel_x_setpoint;
        manaulTx = millis();
    }
    else
    {
        fc_setpoint.angle.pitch = pid_calculate(&pos_pid_obj[POS_VX], fc_setpoint.vel.x, zdrone_state.vel.x, 0, dt);
    }
    if (vel_y_manual)
    {
        fc_setpoint.manual[Y] = true;
        fc_setpoint.angle.roll = vel_y_setpoint;
        manaulTy = millis();
    }
    else
    {
        fc_setpoint.angle.roll = -pid_calculate(&pos_pid_obj[POS_VY], fc_setpoint.vel.y, zdrone_state.vel.y, 0, dt); // Reverse rc ROLL[-1,1] -> ANGLE_ROLL[1,-1]
    }

    if (fc_setpoint.manual[X])
    {
        fc_setpoint.pos.x = zdrone_state.pos.x;
        fc_setpoint.vel.x = 0;
        if (millis() > (manaulTx + 100))
        {
            fc_setpoint.manual[X] = false;
        }
    }
    else
    {
        fc_setpoint.vel.x = pid_calculate(&pos_pid_obj[POS_PX], fc_setpoint.pos.x, zdrone_state.pos.x, 0, dt);
    }

    if (fc_setpoint.manual[Y])
    {
        fc_setpoint.pos.y = zdrone_state.pos.y;
        fc_setpoint.vel.y = 0;
        if (millis() > (manaulTy + 100))
        {
            fc_setpoint.manual[Y] = false;
        }
    }
    else
    {
        fc_setpoint.vel.y = pid_calculate(&pos_pid_obj[POS_PY], fc_setpoint.pos.y, zdrone_state.pos.y, 0, dt);
    }

    if (!FC_STATE_CHECK(FC_ARMED))
    {
        fc_setpoint.pos.y = zdrone_state.pos.y;
        fc_setpoint.pos.x = zdrone_state.pos.x;
        pid_reset_integral(&pos_pid_obj[POS_VX]);
        pid_reset_integral(&pos_pid_obj[POS_VY]);
        pid_reset_integral(&pos_pid_obj[POS_PX]);
        pid_reset_integral(&pos_pid_obj[POS_PY]);
    }

    // F1_wave[0] = fc_optical.quality;
    // F1_wave[1] = fc_optical.raw_x;
    // F1_wave[2] = fc_setpoint.angle.pitch;
    // F1_wave[3] = zdrone_state.vel.x;
    // F1_wave[4] = zdrone_state.pos.x;
    // F1_wave[5] = pos_pid_obj[POS_PX].integral;
    // F1_wave[6] = pos_pid_obj[POS_PX].last_output;
    // F1_wave[7] = pos_pid_obj[POS_VX].integral;
    // F1_wave[8] = pos_pid_obj[POS_VX].last_output;
}
